#include "Sphere.h"
#include "Math.h"
#include "Plane.h"
#include "Rotation.h"

namespace Core
{
	inline rtSphere::rtSphere(void) 
	{
	}

	inline rtSphere::rtSphere(const rtVector3& point) 
	{
		m_Origin = point;
		m_Radius = 0.0f;
	}

	inline rtSphere::rtSphere(const rtVector3& point, const float r) 
	{
		m_Origin = point;
		m_Radius = r;
	}

	inline float rtSphere::operator [] (const int index) const 
	{
		return ((float *)& m_Origin)[index];
	}

	inline float& rtSphere::operator [] (const int index) 
	{
		return ((float *)& m_Origin)[index];
	}

	inline rtSphere rtSphere::operator + (const rtVector3& t) const 
	{
		return rtSphere(m_Origin + t, m_Radius);
	}

	inline rtSphere& rtSphere::operator += (const rtVector3& t) 
	{
		m_Origin += t;
		return *this;
	}

	inline bool rtSphere::compare(const rtSphere& a) const 
	{
		return (m_Origin.compare(a.m_Origin) && m_Radius == a.m_Radius);
	}

	inline bool rtSphere::compare(const rtSphere& a, const float epsilon) const 
	{
		return (m_Origin.compare(a.m_Origin, epsilon) && rtMath::Fabs(m_Radius - a.m_Radius) <= epsilon);
	}

	inline bool rtSphere::operator == (const rtSphere& a) const 
	{
		return compare(a);
	}

	inline bool rtSphere::operator != (const rtSphere& a) const {
		return !compare(a);
	}

	inline void rtSphere::clear(void) 
	{
		m_Origin.zero();
		m_Radius = -1.0f;
	}

	inline void rtSphere::zero(void) 
	{
		m_Origin.zero();
		m_Radius = 0.0f;
	}

	inline void rtSphere::setOrigin(const rtVector3& o) 
	{
		m_Origin = o;
	}

	inline void rtSphere::setRadius(const float r) 
	{
		m_Radius = r;
	}

	inline const rtVector3& rtSphere::getOrigin(void) const 
	{
		return m_Origin;
	}

	inline float rtSphere::getRadius(void) const 
	{
		return m_Radius;
	}

	inline bool rtSphere::isCleared(void) const 
	{
		return (m_Radius < 0.0f);
	}

	inline bool rtSphere::addPoint(const rtVector3& p) 
	{
		if (m_Radius < 0.0f) 
		{
			m_Origin = p;
			m_Radius = 0.0f;
			return true;
		}
		else 
		{
			float r = (p - m_Origin).lengthSqr();
			if (r > m_Radius * m_Radius) 
			{
				r = rtMath::Sqrt(r);
				m_Origin += (p - m_Origin) * 0.5f * (1.0f - m_Radius / r);
				m_Radius += 0.5f * (r - m_Radius);
				return true;
			}
			return false;
		}
	}

	inline bool rtSphere::addSphere(const rtSphere& s) 
	{
		if (m_Radius < 0.0f) {
			m_Origin = s.m_Origin;
			m_Radius = s.m_Radius;
			return true;
		}
		else 
		{
			float r = (s.m_Origin - m_Origin).lengthSqr();
			if (r > (m_Radius + s.m_Radius) * (m_Radius + s.m_Radius)) 
			{
				r = rtMath::Sqrt(r);
				m_Origin += (s.m_Origin - m_Origin) * 0.5f * (1.0f - m_Radius / (r + s.m_Radius));
				m_Radius += 0.5f * ((r + s.m_Radius) - m_Radius);
				return true;
			}
			return false;
		}
	}

	inline rtSphere rtSphere::expand(const float d) const 
	{
		return rtSphere(m_Origin, m_Radius + d);
	}

	inline rtSphere& rtSphere::expandSelf(const float d) 
	{
		m_Radius += d;
		return *this;
	}

	inline rtSphere rtSphere::translate(const rtVector3& translation) const 
	{
		return rtSphere(m_Origin + translation, m_Radius);
	}

	inline rtSphere& rtSphere::translateSelf(const rtVector3& translation) 
	{
		m_Origin += translation;
		return *this;
	}

	inline bool rtSphere::containsPoint(const rtVector3& p) const 
	{
		if ((p - m_Origin).lengthSqr() > m_Radius * m_Radius) 
		{
			return false;
		}
		return true;
	}

	inline bool rtSphere::intersectsSphere(const rtSphere& s) const 
	{
		float r = s.m_Radius + m_Radius;
		if ((s.m_Origin - m_Origin).lengthSqr() > r * r) 
		{
			return false;
		}
		return true;
	}

	inline void rtSphere::fromPointTranslation(const rtVector3& point, const rtVector3& translation) 
	{
		m_Origin = point + 0.5f * translation;
		m_Radius = rtMath::Sqrt(0.5f * translation.lengthSqr());
	}

	inline void rtSphere::fromSphereTranslation(const rtSphere& sphere, const rtVector3& start, const rtVector3& translation) 
	{
		m_Origin = start + sphere.m_Origin + 0.5f * translation;
		m_Radius = rtMath::Sqrt(0.5f * translation.lengthSqr()) + sphere.m_Radius;
	}

	inline void rtSphere::fromPointRotation(const rtVector3& point, const rtRotation& rotation) 
	{
		rtVector3 end = rotation * point;
		m_Origin = (point + end) * 0.5f;
		m_Radius = rtMath::Sqrt(0.5f * (end - point).lengthSqr());
	}

	inline void rtSphere::fromSphereRotation(const rtSphere& sphere, const rtVector3& start, const rtRotation& rotation) 
	{
		rtVector3 end = rotation * sphere.m_Origin;
		m_Origin = start + (sphere.m_Origin + end) * 0.5f;
		m_Radius = rtMath::Sqrt(0.5f * (end - sphere.m_Origin).lengthSqr()) + sphere.m_Radius;
	}

	inline void rtSphere::axisProjection(const rtVector3& dir, float& min, float& max) const 
	{
		float d;
		d = dir * m_Origin;
		min = d - m_Radius;
		max = d + m_Radius;
	}

	float rtSphere::planeDistance(const rtPlane& plane) const 
	{
	float d;

	d = plane.distance(m_Origin);
	if (d > m_Radius) 
	{
		return d - m_Radius;
	}
	if (d < -m_Radius) 
	{
		return d + m_Radius;
	}
	return 0.0f;
}


int rtSphere::planeSide(const rtPlane& plane, const float epsilon) const 
{
	float d;

	d = plane.distance(m_Origin);
	if (d > m_Radius + epsilon) 
	{
		return PLANESIDE_FRONT;
	}
	if (d < -m_Radius - epsilon) 
	{
		return PLANESIDE_BACK;
	}
	return PLANESIDE_CROSS;
}


bool rtSphere::lineIntersection(const rtVector3& start, const rtVector3& end) const 
{
	rtVector3 r, s, e;
	float a;

	s = start - m_Origin;
	e = end - m_Origin;
	r = e - s;
	a = -s * r;
	if (a <= 0) 
	{
		return (s * s < m_Radius * m_Radius);
	}
	else if (a >= r * r) 
	{
		return (e * e < m_Radius * m_Radius);
	}
	else 
	{
		r = s + (a / (r * r)) * r;
		return (r * r < m_Radius * m_Radius);
	}
}



bool rtSphere::rayIntersection(const rtVector3& start, const rtVector3& dir, float& scale1, float& scale2) const 
{
	double a, b, c, d, sqrtd;
	rtVector3 p;

	p = start - m_Origin;
	a = dir * dir;
	b = dir * p;
	c = p * p - m_Radius * m_Radius;
	d = b * b - c * a;

	if (d < 0.0f) {
		return false;
	}

	sqrtd = rtMath::Sqrt(d);
	a = 1.0f / a;

	scale1 = (-b + sqrtd) * a;
	scale2 = (-b - sqrtd) * a;

	return true;
}


void rtSphere::fromPoints(const rtVector3 *points, const int numPoints) 
{
	int i;
	float radiusSqr, dist;
	rtVector3 mins, maxs;

	rtMath::MinMax(mins, maxs, points, numPoints);

	m_Origin = (mins + maxs) * 0.5f;

	radiusSqr = 0.0f;
	for (i = 0; i < numPoints; i++) 
	{
		dist = (points[i] - m_Origin).lengthSqr();
		if (dist > radiusSqr) 
		{
			radiusSqr = dist;
		}
	}
	m_Radius = rtMath::Sqrt(radiusSqr);
}

}

